import numpy as np
from scipy.optimize import curve_fit
import cv2

# 定义拟合函数的模型
def func(x, a, b, c): 
    return a * np.sin(x) + b * np.cos(x) + c

# 打开并读取config.yaml文件
data_file = cv2.FileStorage('config\config.yaml', cv2.FILE_STORAGE_READ)

# 从文件中获取x_data和y_data
num = int(data_file.getNode('num').real())
x_data = []
y_data = []
# print(num)
for i in range(num):
    x_data.append(float(data_file.getNode('theta_' + str(i)).real()))
    y_data.append(data_file.getNode('observed_T_' + str(i)).mat())

# y_data = [np.array(y).flatten() for y in y_data]
# print(y_data)
y_data1 = [y[0][0] for y in y_data]
y_data2 = [y[1][0] for y in y_data]
y_data3 = [y[2][0] for y in y_data]

# 进行曲面拟合
popt1, pcov1 = curve_fit(func, x_data, y_data1)
popt2, pcov2 = curve_fit(func, x_data, y_data2)
popt3, pcov3 = curve_fit(func, x_data, y_data3)

# 输出拟合得到的参数
# print("Optimal parameters 1:", popt1)
# print("Covariance matrix1:", pcov1)
# print("Optimal parameters 2:", popt2)
# print("Covariance matrix2:", pcov2)
# print("Optimal parameters 3:", popt3)
# print("Covariance matrix3:", pcov3)

# 根据拟合的参数，计算拟合后的y_data
y_data1_fit = []
y_data2_fit = []
y_data3_fit = []
for x in x_data:
    y_data1_fit.append(func(x, popt1[0], popt1[1], popt1[2]))
    y_data2_fit.append(func(x, popt2[0], popt2[1], popt2[2]))
    y_data3_fit.append(func(x, popt3[0], popt3[1], popt3[2]))

print(y_data1_fit)
print(y_data2_fit)
print(y_data3_fit)